#ifndef NAV2_CUNSTOM_PLANNERA_HPP
#define NAV2_CUNSTOM_PLANNERA_HPP

#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
// 消息接口
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav_msgs/msg/path.hpp"
// 基类
#include "nav2_core/global_planner.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"

namespace nav2_custom_planner
{
    class CustomPlanner : public nav2_core::GlobalPlanner
    {
    private:
    // 坐标变换缓存指针
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    // 节点指针
    nav2_util::LifecycleNode::SharedPtr node_;
    // 全局代价地图
    nav2_costmap_2d::Costmap2D *costmap_;
    // 全局代价地图坐标系
    std::string global_frame_,name_;
    // 插件分辨率
    double interpolation_resolution_;

    public:
        CustomPlanner() = default;
        ~CustomPlanner() = default;
        // 插件配置方法
        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
        // 插件清除方法
        void cleanup() override;
        // 插件激活方法
        void activate() override;
        // 插件停用方法
        void deactivate() override;
        // 创建路径方法（给定起始、目标点）
        nav_msgs::msg::Path createPlan(
            const geometry_msgs::msg::PoseStamped &start,
            const geometry_msgs::msg::PoseStamped &goal) override;
    };
} // nav2_custom_planner

#endif // NAV2_CUNSTOM_PLANNERA_HPP